#pragma once
#include "Vector.hpp"

//3-variables polynomial function
namespace zzz{

typedef enum {
  POLY3_UNIT,
  POLY3_X, POLY3_Y, POLY3_Z, 
  POLY3_XY, POLY3_XZ, POLY3_YZ,
  POLY3_X2, POLY3_Y2, POLY3_Z2,
  POLY3_X2Y, POLY3_X2Z, POLY3_XY2, POLY3_Y2Z, POLY3_XZ2, POLY3_YZ2, POLY3_XYZ,
  POLY3_X3, POLY3_Y3, POLY3_Z3,
  NUM_POLY3_COEFFS
} POLY3COEFF;


template<typename T>
class Poly3 : public Vector<NUM_POLY3_COEFFS, T>
{
public:
  Poly3():Vector<NUM_POLY3_COEFFS, T>(0){}
  Poly3(const VectorBase<NUM_POLY3_COEFFS, T> &b):Vector<NUM_POLY3_COEFFS, T>(b){}
  Poly3(const Poly3<T> &b):Vector<NUM_POLY3_COEFFS, T>(b){}
  
  using Vector<NUM_POLY3_COEFFS, T>::operator+;
  using Vector<NUM_POLY3_COEFFS, T>::operator-;
  using Vector<NUM_POLY3_COEFFS, T>::operator+=;
  using Vector<NUM_POLY3_COEFFS, T>::operator-=;
  using Vector<NUM_POLY3_COEFFS, T>::Data;

  Poly3<T> Multiply(const Poly3<T> &b) const
  {
    Poly3<T> r;

#if 0
    if (!Poly3MultCheck((*this), b)) 
    {
      zout<<"[poly3_mult] Polynomials cannot be multiplied!\n";
      return r;
    }
#endif

    const T *v=Data();
    r[POLY3_UNIT] = v[POLY3_UNIT] * b[POLY3_UNIT];
    r[POLY3_X] = v[POLY3_X] * b[POLY3_UNIT] + v[POLY3_UNIT] * b[POLY3_X];
    r[POLY3_Y] = v[POLY3_Y] * b[POLY3_UNIT] + v[POLY3_UNIT] * b[POLY3_Y];
    r[POLY3_Z] = v[POLY3_Z] * b[POLY3_UNIT] + v[POLY3_UNIT] * b[POLY3_Z];

    r[POLY3_XY] = v[POLY3_XY] * b[POLY3_UNIT] + v[POLY3_UNIT] * b[POLY3_XY] + v[POLY3_X] * b[POLY3_Y] + v[POLY3_Y] * b[POLY3_X];
    r[POLY3_XZ] = v[POLY3_XZ] * b[POLY3_UNIT] + v[POLY3_UNIT] * b[POLY3_XZ] + v[POLY3_X] * b[POLY3_Z] + v[POLY3_Z] * b[POLY3_X];
    r[POLY3_YZ] = v[POLY3_YZ] * b[POLY3_UNIT] + v[POLY3_UNIT] * b[POLY3_YZ] + v[POLY3_Y] * b[POLY3_Z] + v[POLY3_Z] * b[POLY3_Y];

    r[POLY3_X2] = v[POLY3_X2] * b[POLY3_UNIT] + v[POLY3_UNIT] * b[POLY3_X2] + v[POLY3_X] * b[POLY3_X];
    r[POLY3_Y2] = v[POLY3_Y2] * b[POLY3_UNIT] + v[POLY3_UNIT] * b[POLY3_Y2] + v[POLY3_Y] * b[POLY3_Y];
    r[POLY3_Z2] = v[POLY3_Z2] * b[POLY3_UNIT] + v[POLY3_UNIT] * b[POLY3_Z2] + v[POLY3_Z] * b[POLY3_Z];

    r[POLY3_X2Y] = v[POLY3_X2Y] * b[POLY3_UNIT] + v[POLY3_UNIT] * b[POLY3_X2Y] + 
      v[POLY3_X2] * b[POLY3_Y] + v[POLY3_Y] * b[POLY3_X2] + 
      v[POLY3_XY] * b[POLY3_X] + v[POLY3_X] * b[POLY3_XY];

    r[POLY3_X2Z] = v[POLY3_X2Z] * b[POLY3_UNIT] + v[POLY3_UNIT] * b[POLY3_X2Z] + 
      v[POLY3_X2] * b[POLY3_Z] + v[POLY3_Z] * b[POLY3_X2] + 
      v[POLY3_XZ] * b[POLY3_X] + v[POLY3_X] * b[POLY3_XZ];

    r[POLY3_XY2] = v[POLY3_XY2] * b[POLY3_UNIT] + v[POLY3_UNIT] * b[POLY3_XY2] + 
      v[POLY3_X] * b[POLY3_Y2] + v[POLY3_Y2] * b[POLY3_X] + 
      v[POLY3_XY] * b[POLY3_Y] + v[POLY3_Y] * b[POLY3_XY];

    r[POLY3_Y2Z] = v[POLY3_Y2Z] * b[POLY3_UNIT] + v[POLY3_UNIT] * b[POLY3_Y2Z] + 
      v[POLY3_Y2] * b[POLY3_Z] + v[POLY3_Z] * b[POLY3_Y2] + 
      v[POLY3_YZ] * b[POLY3_Y] + v[POLY3_Y] * b[POLY3_YZ];

    r[POLY3_XZ2] = v[POLY3_XZ2] * b[POLY3_UNIT] + v[POLY3_UNIT] * b[POLY3_XZ2] + 
      v[POLY3_X] * b[POLY3_Z2] + v[POLY3_Z2] * b[POLY3_X] + 
      v[POLY3_XZ] * b[POLY3_Z] + v[POLY3_Z] * b[POLY3_XZ];

    r[POLY3_YZ2] = v[POLY3_YZ2] * b[POLY3_UNIT] + v[POLY3_UNIT] * b[POLY3_YZ2] + 
      v[POLY3_Y] * b[POLY3_Z2] + v[POLY3_Z2] * b[POLY3_Y] + 
      v[POLY3_YZ] * b[POLY3_Z] + v[POLY3_Z] * b[POLY3_YZ];

    r[POLY3_XYZ] = v[POLY3_XYZ] * b[POLY3_UNIT] + v[POLY3_UNIT] * b[POLY3_XYZ] + 
      v[POLY3_XY] * b[POLY3_Z] + v[POLY3_Z] * b[POLY3_XY] + 
      v[POLY3_XZ] * b[POLY3_Y] + v[POLY3_Y] * b[POLY3_XZ] + 
      v[POLY3_YZ] * b[POLY3_X] + v[POLY3_X] * b[POLY3_YZ];

    r[POLY3_X3] = v[POLY3_X3] * b[POLY3_UNIT] + v[POLY3_UNIT] * b[POLY3_X3] + 
      v[POLY3_X2] * b[POLY3_X] + v[POLY3_X] * b[POLY3_X2];

    r[POLY3_Y3] = v[POLY3_Y3] * b[POLY3_UNIT] + v[POLY3_UNIT] * b[POLY3_Y3] + 
      v[POLY3_Y2] * b[POLY3_Y] + v[POLY3_Y] * b[POLY3_Y2];

    r[POLY3_Z3] = v[POLY3_Z3] * b[POLY3_UNIT] + v[POLY3_UNIT] * b[POLY3_Z3] + 
      v[POLY3_Z2] * b[POLY3_Z] + v[POLY3_Z] * b[POLY3_Z2];

    return r;
  }

  //multiply degree 0 1 of this to degree 0 1 of b
  Poly3<T> Multiply11(const Poly3<T> &b)
  {
    Poly3<T> r;
    T *v=Data();

    r[POLY3_UNIT] = v[POLY3_UNIT] * b[POLY3_UNIT];
    r[POLY3_X] = v[POLY3_X] * b[POLY3_UNIT] + v[POLY3_UNIT] * b[POLY3_X];
    r[POLY3_Y] = v[POLY3_Y] * b[POLY3_UNIT] + v[POLY3_UNIT] * b[POLY3_Y];
    r[POLY3_Z] = v[POLY3_Z] * b[POLY3_UNIT] + v[POLY3_UNIT] * b[POLY3_Z];

    r[POLY3_XY] = v[POLY3_X] * b[POLY3_Y] + v[POLY3_Y] * b[POLY3_X];
    r[POLY3_XZ] = v[POLY3_X] * b[POLY3_Z] + v[POLY3_Z] * b[POLY3_X];
    r[POLY3_YZ] = v[POLY3_Y] * b[POLY3_Z] + v[POLY3_Z] * b[POLY3_Y];

    r[POLY3_X2] = v[POLY3_X] * b[POLY3_X];
    r[POLY3_Y2] = v[POLY3_Y] * b[POLY3_Y];
    r[POLY3_Z2] = v[POLY3_Z] * b[POLY3_Z];

    return r;
  }

  //multiply degree 0 1 2 of this to degree 0 1 of b
  Poly3<T> Multiply21(const Poly3<T> &b) 
  {
    Poly3<T> r;
    T *v=Data();

    r[POLY3_UNIT] = v[POLY3_UNIT] * b[POLY3_UNIT];
    r[POLY3_X] = v[POLY3_X] * b[POLY3_UNIT] + v[POLY3_UNIT] * b[POLY3_X];
    r[POLY3_Y] = v[POLY3_Y] * b[POLY3_UNIT] + v[POLY3_UNIT] * b[POLY3_Y];
    r[POLY3_Z] = v[POLY3_Z] * b[POLY3_UNIT] + v[POLY3_UNIT] * b[POLY3_Z];

    r[POLY3_XY] = v[POLY3_XY] * b[POLY3_UNIT] + v[POLY3_X] * b[POLY3_Y] + v[POLY3_Y] * b[POLY3_X];
    r[POLY3_XZ] = v[POLY3_XZ] * b[POLY3_UNIT] + v[POLY3_X] * b[POLY3_Z] + v[POLY3_Z] * b[POLY3_X];
    r[POLY3_YZ] = v[POLY3_YZ] * b[POLY3_UNIT] + v[POLY3_Y] * b[POLY3_Z] + v[POLY3_Z] * b[POLY3_Y];

    r[POLY3_X2] = v[POLY3_X2] * b[POLY3_UNIT] + v[POLY3_X] * b[POLY3_X];
    r[POLY3_Y2] = v[POLY3_Y2] * b[POLY3_UNIT] + v[POLY3_Y] * b[POLY3_Y];
    r[POLY3_Z2] = v[POLY3_Z2] * b[POLY3_UNIT] + v[POLY3_Z] * b[POLY3_Z];

    r[POLY3_X2Y] = 
      v[POLY3_X2] * b[POLY3_Y] + 
      v[POLY3_XY] * b[POLY3_X];

    r[POLY3_X2Z] = 
      v[POLY3_X2] * b[POLY3_Z] + 
      v[POLY3_XZ] * b[POLY3_X];

    r[POLY3_XY2] = 
      v[POLY3_Y2] * b[POLY3_X] + 
      v[POLY3_XY] * b[POLY3_Y];

    r[POLY3_Y2Z] = 
      v[POLY3_Y2] * b[POLY3_Z] + 
      v[POLY3_YZ] * b[POLY3_Y];

    r[POLY3_XZ2] = 
      v[POLY3_Z2] * b[POLY3_X] + 
      v[POLY3_XZ] * b[POLY3_Z];

    r[POLY3_YZ2] = 
      v[POLY3_Z2] * b[POLY3_Y] + 
      v[POLY3_YZ] * b[POLY3_Z];

    r[POLY3_XYZ] = 
      v[POLY3_XY] * b[POLY3_Z] +
      v[POLY3_XZ] * b[POLY3_Y] +
      v[POLY3_YZ] * b[POLY3_X];

    r[POLY3_X3] = v[POLY3_X2] * b[POLY3_X];
    r[POLY3_Y3] = v[POLY3_Y2] * b[POLY3_Y];
    r[POLY3_Z3] = v[POLY3_Z2] * b[POLY3_Z];

    return r;
  }

  T Eval(const T x, const T y, const T z) {
    T *v=Data();
    double r = v[POLY3_UNIT];

    r += v[POLY3_X] * x;
    r += v[POLY3_Y] * y;
    r += v[POLY3_Z] * z;

    r += v[POLY3_XY] * x * y;
    r += v[POLY3_XZ] * x * z;
    r += v[POLY3_YZ] * y * z;

    r += v[POLY3_X2] * x * x;
    r += v[POLY3_Y2] * y * y;
    r += v[POLY3_Z2] * z * z;

    r += v[POLY3_X2Y] * x * x * y;
    r += v[POLY3_X2Z] * x * x * z;
    r += v[POLY3_XY2] * x * y * y;
    r += v[POLY3_Y2Z] * y * y * z;
    r += v[POLY3_XZ2] * x * z * z;
    r += v[POLY3_YZ2] * y * z * z;
    r += v[POLY3_XYZ] * x * y * z;

    r += v[POLY3_X3] * x * x * x;
    r += v[POLY3_Y3] * y * y * y;
    r += v[POLY3_Z3] * z * z * z;

    return r;
  }

  //degree of highest non-zero coeff
  int Degree() const
  {
    T *v=Data();
    if (v[POLY3_X3] != 0.0 || v[POLY3_Y3] != 0.0 || v[POLY3_Z3] != 0.0 ||
        v[POLY3_X2Y] != 0.0 || v[POLY3_X2Z] != 0.0 || v[POLY3_XY2] != 0.0 || 
        v[POLY3_XZ2] != 0.0 || v[POLY3_Y2Z] != 0.0 || v[POLY3_YZ2] != 0.0 ||
        v[POLY3_XYZ] != 0.0)
        return 3;

    if (v[POLY3_X2] != 0.0 || v[POLY3_Y2] != 0.0 || v[POLY3_Z2] != 0.0 ||
        v[POLY3_XY] != 0.0 || v[POLY3_XZ] != 0.0 || v[POLY3_YZ] != 0.0)
        return 2;

    if (v[POLY3_X] != 0.0 || v[POLY3_Y] != 0.0 || v[POLY3_Z] != 0.0)
        return 1;

    return 0;
  }



};

template<typename T>
int Poly3MultCheck(Poly3<T> a, Poly3<T> b) 
{
  int deg_a = a.Degree();
  int deg_b = b.Degree();

  if (deg_a + deg_b <= 3)
    return 1;
  else
    return 0;
}



}

